
#include <ctl_api.h>
#include <stdio.h>
#include "freeRTOS.h"
#include "task.h"
#include "Vector.h"

extern "C"
{
#include <string.h>
#include "main.h"
#include "Logomatic_V2.h"
#include "ADC.h"
//#include "i2c.h"

#if LOG_DATA
#include "fat16.h"
#include "rootdir.h"
#include "sd_raw.h"
#endif
}

#define BUFFER_LENGTH       4
#define ACCEL_THRESHOLD     50.0f
#define MIN_THRESHOLD       10.0f
#define ERROR_THRESHOLD     5.0f
#define MESG_LENGTH         6

#define TASK_STACK_SIZE             300
#define PID_TASK_PRIORITY           4
#define Timer_1ms_TASK_PRIORITY     5
#define LOGGING_TASK_PRIORITY       3
#define COUNTING_TASK_PRIORITY      2


//const unsigned long crp __attribute__ ((at(0x2FC))) = 0x87654321;


// TILT SENSOR COMMAND 
#define   MEAS            0b00000000
#define   RWTR            0b00001000
#define   RDSR            0b00001010
#define   RLOAD           0b00001011
#define   STX             0b00001110
#define   STY             0b00001111
#define   RDAX            0b00010000
#define   RDAY            0b00010001

#define PINSEL0_TXD0 0b01
#define PINSEL0_RXD0 (0b01<<2)
#define PINSEL0_TXD1 (0b01<<16)
#define PINSEL0_RXD1 (0b01<<18)

int tilt_x=1024;
int tilt_y=1024;
// spi1 
#define  spi_cs   20
#define  spi_miso 18
#define  spi_mosi 19
#define  spi_clk  17

////i2c
#define PINSEL0_SCL0 (0B01<<4)
#define PINSEL0_SDA0 (0b01<<6)
#define PINSEL0_SCL1 (0B11<<22)
#define PINSEL0_SDA1 (0b11<<28)

#define acc_add 0b00111000
#define gyro_add 0b11010000

#define OUT_X_LSB 0x05
#define OUT_X_MSB 0x06
#define OUT_Y_LSB 0x07 
#define OUT_Y_MSB 0x08
#define OUT_Z_LSB 0x09 
#define OUT_Z_MSB 0x0A
#define OUT_X_GYRO 0xA8

#define CTRL_REG1 0x38
#define ACC_XYZ_DATA_CFG 0x16

#define acc_init 0b00000001  //50hz +-2g

#define GREEN_LED 11//30
#define RED_LED 30//11

void i2c_init(void);
void i2c_acc_init(void);
void i2c_gyro_init(void);
void get_acc_data(void);
void get_gyro_data(void);
//float get_acc_z(void);
void I2CISR_handler(void);
float acc_cal_x(void);
float acc_cal_z(void);
float acc_cal_y(void);
void data_cal(void);

unsigned I2CAddress;
unsigned I2C_reg_Address;
unsigned I2CData;
#define read_gyro 1
#define read_acc 0
#define data_length 6
char i2cact=0;
char write_count=0;
char read_count=0;
char multi_read=0;
int acc_data_sum[data_length];
char acc_data_mean[data_length];
char acc_data[data_length];

char gyro_data[data_length];
char gyro_data_mean[data_length];
int gyro_data_sum[data_length];
char data_count=0;
int acc_x=0;
int acc_y=0;
int acc_z=0;
int gyro_x=0;
int gyro_y=0;
int gyro_z=0;
char i2c_bus_busy=0;
char transientEvent = 0;
bool lock_transientEvent = false;
char transientEventLockoutTimer = 0;
#define  GYRO_BUFF_SIZE 10
    int gyro_buff_index=0;
   int16_t gyro_buff[GYRO_BUFF_SIZE];
    int32_t gyro_buffSum=0;
    int32_t gyro_avg=0;
    #define GYRO_MAX_THRESH 2500
    #define GYRO_MIN_THRESH -2500
    #define GYRO_DIFF_THRESH 4000
    #define GYRO_NEG_SUM_THRESH -4000
    #define GYRO_POS_SUM_THRESH -4000
  
  int32_t gyro_buffMin = 0;
  int32_t gyro_buffMax = 0;
  int32_t gyro_buffDiff = 0;

//i2c end
static int CRC(unsigned  char* msg, int num);

const int TICKS_PER_REVOLUTION = 2500;    //90/26*360;    // number of ticks per revolution of the wheel  both rising and falling edge
const float WHEEL_RADIUS = 3.9;           // radius of the wheel (in inches)

const float BRAKE_PROP  = 1.04f;
const float PUSH_PROP   = 0.9f;
float speed_prop = 2.0f;
float curr_prop = 0.0f;
float prev_prop = 0.0f;
float curr_prop_sw = 0.0f;
float speed_prop_sw = 0.0f;

float _speed = 0.0f;            // current speed (initial=0)

int manual_step=40;
int manual_output=0;
int dac_output=0;
char mesg0[MESG_LENGTH];
char mesg1[MESG_LENGTH];

/***************************/
/* PID Control Variables   */
/***************************/
  float _percent_error = 0.0f;
  float error_percent[4];
  unsigned int pid_delay=0;
  unsigned int push_delay=0;
  
  float previous_error = 0.0f;
  float integral=0,derivative=0,error=0;
  float output = 0.0f;
  unsigned int duty = 0;
  unsigned int duty_stable = 0;
  bool stable=false;
  bool pip=false;

static const int HIGH_COAST_TIME=90;
static const int NORMAL_COAST_TIME=60;
static const int LOW_COAST_TIME=30;
static int COAST_TIME=NORMAL_COAST_TIME;        // defined in 10ths of a second (50=5seconds)
static int coasting_time = 0;

static const int AUTO_TIME_DELAY  = 10;         // time after releasing FSR before auto mode kicks in (defined in 10ths of a second)
int DELAY_AFTER_MANUAL = 0;

static const int MANUAL_MODE  = 1;
static const int AUTO_MODE    = 0;
static const int OFF_MODE     = 2;
static int DRIVE_MODE = MANUAL_MODE;

static const int sw_press  = 1;
static const int sw_release  = 0;
static const int sw_coast  = 2;
static const int sw_stop  = 3;

int sw_state=sw_stop;
int sw_mode=sw_release;
int sw_pre_mode= sw_release;

bool push_detected = false;
int push_detected_delay=0;
int push_start_delay=0;
int _adcval;
int pre_adcval;
int pressed=0;
int pressed_delay=0;

/***************************/
/*Push Detection Variables */
/***************************/
  unsigned int cap_val=0;
  unsigned int prev_cap_val = 0;
  signed int accel_vals[BUFFER_LENGTH];
  float filtered_vals[BUFFER_LENGTH];
  float filtered_DD[BUFFER_LENGTH];
  signed int diff[BUFFER_LENGTH];
  signed int doublediff[BUFFER_LENGTH];

  float filter_acc=0;
  float pre_acc=0;
  float pre_speed=0;
  float filter_dd=0;
  float ratio=0;
  float log_speed=0;

//butter filter
  float filter_b[5]={0.0004f,0.0017f,0.0025f,0.0017f,0.0004f};
  float filter_a[5]={1.0000f,-3.1806f,3.8612f,-2.1122f,0.4383f};
  float filter_z[5]={0,0,0,0,0};
//  float filter_b[3]={ 0.0413f,0.0825f, 0.0413f};
//  float filter_a[3]={1.0000f,   -1.3490f,    0.5140f};
//  float filter_z[3]={0,0,0};
  float filter_x=0;
  float filter_y=0;
  float filter_speed=0;

#if LOG_DATA
  struct fat16_file_struct* handle;
  char name[32];
  char _str1[50];      // used for writing to the uSD card
#endif

typedef struct
{
   uint32_t frequency;            // Blinking frequency
} sTaskParams_t, *spTaskParams_t;

typedef struct
{
   float Kp,          // proportional constant
         Ki,          // integral constant
         Kd;          // derivative constant
   float setpoint;    // setpoint for PID
   uint32_t frequency;// frequency for task
} sPIDParams_t, *spPIDParams_t;

sTaskParams_t CountingArgs = {
                            20    // Task Execution Frequency
                            };
sTaskParams_t Timer1msArgs =    {
                            1000    // Task Execution Frequency
                            };
sPIDParams_t PIDArgs =      {
                            10,   // Kp
                            6,    // Ki
                            2,    // Kd
                            0,    // initial speed setpoint
                            20    // Task Execution Frequency
                            };

extern "C"
{
  int           main              ( void );
  void          bootup            ( void );
  void          PllFeed           ( void );
  static void   initTimer1        ( void );
  void          spi_init          ( void );
  int           spi_receive_x       ( void );
  int           spi_receive_y       ( void );
  void          spi_trans         (char DATA);
  void          spi_sw_init       ( void );
  int           spi_sw_receive_x  ( void );
  int           spi_sw_receive_y  ( void );
  void          _wait_1us         ( void );
  void          uart0_tx          (char data);
  void          uart1_tx          (char data);

#if LOG_DATA
  void          card_init         ( void );
  void          log_string        ( char *buf );
#endif

}

char button_longhold=0;
char cruse=0;

xTaskHandle xCountingTaskHandle,
            xPIDTaskHandle,
            xTimer_1msHandle;
static void   CountingTask        ( void *pvParameters );
static void   PIDTask             ( void *pvParameters );
static void   Timer_1msTask       ( void *pvParameters );

#if LOG_DATA
  xTaskHandle xLoggingTaskHandle;
  sTaskParams_t LoggingArgs = {
                              10    // Task Execution Frequency
                              }; 
  static void   LoggingTask         ( void *pvParameters );
#endif

void          readSwitches        ( void );

void          vTimer1ISRWrapper   ( void ) __attribute__ ( ( naked ) );
void          vTimer1Handler      ( void ) __attribute__ ( ( noinline ) );
void          UNDEF_Routine       ( void ) __attribute__ ( ( interrupt ( "UNDEF" ) ) );

extern "C" void abort( void )
{
  while (1 == 1)
  {}
} // abort


extern "C" void bootup( void )
{
portENTER_CRITICAL();
{
    SCS=3;

    // Setting Multiplier and Divider values
    PLLCFG=0x25; // M = 6
    PllFeed();
    
    // Enabling the PLL */
    PLLCON=0x1;
    PllFeed();
    
#define PLOCK 0x400
    // Wait for the PLL to lock to set frequency
    while(!(PLLSTAT & PLOCK)) ;
    
    // Connect the PLL as the clock source
    PLLCON=0x3;
    PllFeed();
    
    // Enabling MAM and setting number of clocks used for Flash memory fetch (4 cclks in this case)
    //MAMTIM=0x3; //VCOM?
    MAMCR=0x2;
    MAMTIM=0x4; //Original
    
    // Setting peripheral Clock (pclk) to System Clock (cclk)
    VPBDIV=0x1;

    P0DIR |= (1<<STAT1)|(1<<STAT0);
    
    PINSEL1 |= (2<<18);  // AOUT is on P0.25 (logomatic P4)

    PINSEL0 = 0;
    P0DIR &= ~(3);
    P0DIR |= (1<<GREEN_LED)|(1<<RED_LED);
    P0SET =(1<<GREEN_LED)|(1<<RED_LED);
}
portEXIT_CRITICAL();
      
    //LogomaticV2Stat(STAT0, LOGOMATIC_STAT_ON);
    //LogomaticV2Stat(STAT1, LOGOMATIC_STAT_ON);
}

extern "C" void PllFeed( void )
{
    PLLFEED=0xAA;
    PLLFEED=0x55;
}

void uart0ISR (void)
{
  char data;
  switch(U0IIR& 0x0f)
  {
    case 0b0100:
    {
      data=U0RBR&0xFF;
    }
    break;

    default:
    break;
  }
}

void uart1ISR (void)
{
  char data;
  switch(U1IIR& 0x0f)
  {
    case 0b0100:
    {
      data=U1RBR&0xFF;
    }
    break;

    default:
    break;
  }
}

extern "C" void uart_init (void)
{
   #define DLL_9600_BAUD 170
   #define DLM_9600_BAUD 1
   #define FDR_9600_BAUD (1 | (10<<4))
   #define DLL_38400_BAUD 74
   #define DLM_38400_BAUD 0
   #define FDR_38400_BAUD (7 | (12<<4))
   PINSEL0|= PINSEL0_TXD0|PINSEL0_RXD0|PINSEL0_TXD1|PINSEL0_RXD1; 
   U0LCR |= 0B11;    //8 BIT CHAR  1 STOP BIT NO PARITY
   U0LCR |= 1<<7;    // DLAB=1
   U0DLL = 74;
   U0DLM = 0;
   U0FDR = 7 | (12<<4);   //BAUD RATE 38400
  
  // U0DLL = 170;
  // U0DLM = 1;
  // U0FDR = 1 | (10<<4);   //BAUD RATE 9600
   U0LCR &= ~(1<<7);   //DLAB =0
   U0FCR |= 1|(1<<1)|(1<<2); // ENABLE AND CLEAR FIFO
   U0TER = 1<<7;  //ENABLE TRANSMIT
  // U0IER |= 1<<0; //RDA INTERRUPT ENABLE
  
   U1LCR |= 0B11;    //8 BIT CHAR  1 STOP BIT NO PARITY
   U1LCR |= 1<<7;    // DLAB=1
   U1DLL = 74;
   U1DLM = 0;
   U1FDR = 7 | (12<<4);   //BAUD RATE 38400
  // U1DLL = 170;
  // U1DLM = 1;
  // U1FDR = 1 | (10<<4);   //BAUD RATE 9600
   U1LCR &= ~(1<<7);   //DLAB =0
   U1FCR |= 1|(1<<1)|(1<<2); // ENABLE AND CLEAR FIFO
   U1TER = 1<<7;  //ENABLE TRANSMIT
  // U1IER |= 1<<0; //RDA INTERRUPT ENABLE
  
   // ISR SETTINGS
  // VICIntSelect &= ~( 1<<6 );  //UART0 
  // VICVectCntl0 = 0x20 | 6;    //UART0
  // VICVectCntl1 = 0x20 | 7;    //UART1
  // VICVectAddr0 = (unsigned int)uart0ISR;
  // VICVectAddr1 = (unsigned int)uart1ISR;
  // VICIntEnable |= ( 1<<6 )|(1<<7);   // ISR ENABLE
}

static int CRC(char* msg,int len)
{
    int crc = 65535;
    int i,j;
    for (i=0;i<len;i++)
    {
        crc ^= msg[i];
        for (j=0;j<8;j++)
        { 
            if ( !(crc & 0x01) )
            {
                crc = crc>>1;
                crc &= 0x7FFF;
            }
            else 
            {
                crc = crc>>1;
                crc &= 0x7FFF;
                crc ^= 0xA001;
            }
        }
    }
    return crc;
}

extern "C" void uart0_tx_array (char* data, char length)
{ 
  int i=0;
  for (i=0;i<length;i++)
    {
      //    uart0_tx(0x12); 
      //    uart0_tx(0x34); 
      //    uart0_tx(i); 
      uart0_tx(data[i]&0xff); 
      //    vTaskDelay(5);
      //    uart0_tx(0x00); 
    }
}

extern "C" void uart1_tx_array (char* data, char length)
{ 
  int i=0;
  for (i=0;i<length;i++)
    {
      uart1_tx(data[i]&0xff);
      //    vTaskDelay(5);
      //    _wait_1us();
      //    uart1_tx(0xff); 
    }
}

extern "C" void uart0_tx (char data)
{
while(U0LSR|(1<<5)) //THR IS EMPTY
  {
    U0THR=data;
    break;
  }
}

extern "C" void uart1_tx (char data)
{
  portENTER_CRITICAL ();
  while(U1LSR|(1<<5)) //THR IS EMPTY
  {
    U1THR=data;
    break;
  }
  portEXIT_CRITICAL (); 
  vTaskDelay(1);
}

extern "C" void spi_sw_init (void)
{
    /* Configure Pin Connect Block */
    //PINSEL1 |= (1<<3)|(1<<5)|(1<<7);//|(1<<9); 
    P0DIR |= (1<<spi_cs)|(1<<spi_mosi)|(1<<spi_clk); 
    P0CLR = 1<<spi_clk;
    P0SET = 1<<20;
     /* Set pclk to same as cclk */
    //VPBDIV=0x1;
    //SSPCPSR=0X20;    // CLK/64   12M/64=187.5K
    //S0SPCCR=0x8;
//    SSPCR0= 0B1111 | (31<<8)|(0<<6)|(0<<7);    //8bit  //1111=16bit
//    P0SET = 1<<20;
//    SSPCR1=1<<1;     //MSATER MODE & ENABLE SSP
    /* Device selected as master */
    //S0SPCR=0x20;
}

extern "C" void spi_init (void)
{
    /* Configure Pin Connect Block */
    PINSEL1 |= (1<<3)|(1<<5)|(1<<7);//|(1<<9); 
    P0DIR |= 1<<20; 
     /* Set pclk to same as cclk */
    //VPBDIV=0x1;
    SSPCPSR=0X20;    // CLK/64   12M/64=187.5K
    //S0SPCCR=0x8;
    SSPCR0= 0B1111 | (31<<8)|(0<<6)|(0<<7);    //8bit  //1111=16bit
    P0SET = 1<<20;
    SSPCR1=1<<1;     //MSATER MODE & ENABLE SSP
    /* Device selected as master */
    //S0SPCR=0x20;
}

extern "C" void spi_trans (char DATA)
{
    P0CLR = 1<<20;
    /* Write data out */
    //LogomaticV2Stat(STAT0, LOGOMATIC_STAT_OFF);
    SSPDR=(DATA<<8);
    /* Wait for transfer to be completed */
    while((SSPSR & (1<<4)))
    {}   
    P0SET = 1<<20;
}

extern "C" int spi_sw_receive_y (void)
{
    P0CLR = 1<<spi_cs;
    int _result=0;
    char _data;
    //LogomaticV2Stat(STAT0, LOGOMATIC_STAT_ON);
    _data=RDAY;
    int i=8;
    while(i>0)  //write 8 bit data
    {
      P0CLR = 1<<spi_clk;
      if (_data & 0x80)
        P0SET = 1<< spi_mosi;
        else
        P0CLR = 1 << spi_mosi;

      _data <<=1;
      _wait_1us();
      _wait_1us();
      _wait_1us();
      P0SET= 1 << spi_clk;
      _wait_1us();
      _wait_1us();
      _wait_1us();
      i--;
    }
    i=11;    // read 11 bit data
    while (i>0)
    {
      P0CLR = 1<<spi_clk;
      _wait_1us();
      _wait_1us();
      _wait_1us();
      P0SET = 1<<spi_clk;
      _wait_1us();
      _wait_1us();
      _wait_1us();
      _result<<=1;
      if(P0PIN & (1 << spi_miso))
      _result++;
      i--;   
    }
    P0CLR = 1<<spi_clk;
    P0SET = 1<<20;
    return _result;
}

extern "C" int spi_sw_receive_x (void)
{
    P0CLR = 1<<spi_cs;
    int _result=0;
    char _data;
    //LogomaticV2Stat(STAT0, LOGOMATIC_STAT_ON);
    _data=RDAX;
    int i=8;
    while(i>0)  //write 8 bit data
    {
      P0CLR = 1<<spi_clk;
      if (_data & 0x80)
        P0SET = 1<< spi_mosi;
        else
        P0CLR = 1 << spi_mosi;

      _data <<=1;
      _wait_1us();
      _wait_1us();
      _wait_1us();
      P0SET= 1 << spi_clk;
      _wait_1us();
      _wait_1us();
      _wait_1us();
      i--;
    }
    i=11;    // read 11 bit data
    while (i>0)
    {
      P0CLR = 1<<spi_clk;
      _wait_1us();
      _wait_1us();
      _wait_1us();
      P0SET = 1<<spi_clk;
      _wait_1us();
      _wait_1us();
      _wait_1us();
      _result<<=1;
      if(P0PIN & (1 << spi_miso))
      _result++;
      i--;    
    }
    P0CLR = 1<<spi_clk;
    P0SET = 1<<20;
    return _result;
}

extern "C" void _wait_1us (void)
{
  int i=0;
  for (i=0;i<10;i++)
  {}
}

extern "C" int spi_receive_x (void)
{
    P0CLR = 1<<20;
    int _result;
    int _temp,_temp1;
    //LogomaticV2Stat(STAT0, LOGOMATIC_STAT_ON);
    SSPDR=(RDAX*256);
    /* Wait for transfer to be completed */
    while((SSPSR & (1<<4)))
    {}
    _temp=SSPDR&0xff;
//    /* Write data out */
//    SSPDR=0xff;  //dummy byte
//    /* Wait for transfer to be completed */
//    while((SSPSR & (1<<4))) // receive data
//    {}
//    _temp=SSPDR&0xff;
    
    SSPDR=0xff;  //dummy byte
    /* Wait for transfer to be completed */
    while((SSPSR & (1<<4))) // receive data
    {}
    _temp1=SSPDR;
    _result=_temp*8+_temp1/32;
    P0SET = 1<<20;
    return _temp;
}

extern "C" int spi_receive_y (void)
{
    P0CLR = 1<<20;
    int _result=0;
    int _temp=0,_temp1=0;
   // LogomaticV2Stat(STAT0, LOGOMATIC_STAT_ON);

    SSPDR=(char)RDAY;
    /* Wait for transfer to be completed */
    while((SSPSR & (1<<4)))
    {}
  //  P0SET = 1<<20;
  //  P0CLR = 1<<20;

    /* Write data out */
    SSPDR=0xff;  //dummy byte
    /* Wait for transfer to be completed */
    while((SSPSR & (1<<4))) // receive data
    {}
    _temp=SSPDR&0xff;
    //P0SET = 1<<20;
    //P0CLR = 1<<20;

    SSPDR=0xff;  //dummy byte
    /* Wait for transfer to be completed */
    while((SSPSR & (1<<4))) // receive data
    {}
    _temp1=SSPDR&0xff;
    _result=_temp*8+_temp1/32;
    P0SET = 1<<20;
    return _result;
}

extern "C" void initTimer1( void )
{
    PINSEL1 |= (3<<10);           // enable cap1.3 on p0.21 (pin P6 on logomatic v2)

    T1TCR = (1<<1);                             // disable and reset Timer0
    T1CTCR = (3<<2)|(3<<0);                     // count using CAP1.3 as the counter
    T1TCR = 1;                                  // enable the timer
}

#if LOG_DATA
extern "C" void card_init( void )
{
	if(!sd_raw_init()) //Attempt to init SD raw mode
                while (1);

	if(openroot()) //Attempt to open root directory
		while (1);
}

extern "C" void log_string( char *buf )
{
	int stringSize = strlen(buf);
	fat16_write_file(handle, (char*)buf, stringSize);
	sd_raw_sync();
}

void LoggingTask( void * pvParameters )
{
    sTaskParams_t *  p_Args = reinterpret_cast<sTaskParams_t *>(pvParameters);

    int count = 0;
    sprintf(name,"TCK%02d.csv",count);
    while(root_file_exists(name))
    {
        count++;
        sprintf(name,"TCK%02d.csv",count);
    }
    handle = root_open_new(name);  
    sd_raw_sync();    
    log_string((char *)"speed,setpoint,curr_prop,speedprop,push_start_delay,filtered_vals\n");

    while(true)
    {
        sprintf(_str1,"%0.3f,%0.3f,%0.3f,%0.3f,%d,%0.3f\n",_speed,PIDArgs.setpoint,curr_prop,speed_prop,push_start_delay,filtered_vals[BUFFER_LENGTH -1]);
        log_string(_str1);
        vTaskDelay(configTICK_RATE_HZ / (p_Args->frequency));
    }
}
#endif

void readSwitches( void )
{
    int pinval = P0PIN;
    if(pinval&(1<<10))
    {
        sw_mode=sw_press;
    }
    else
    {
        sw_mode=sw_release;
    }
    if(button_longhold>0)
    {
      button_longhold--;
    }
    if((sw_mode==sw_press) && (sw_pre_mode==sw_release))  // switch pressed
    {
      pressed_delay=0;
      cruse=0;
    }
    if((sw_mode==sw_press) && (sw_pre_mode==sw_press))  // switch hold  
    {
      pressed_delay++;
    }
    if((sw_mode==sw_release) && (sw_pre_mode==sw_press)) // releasing
    {   
        if(pressed_delay<7 && pressed_delay>0)
        {
          pressed_delay=0;
          if(button_longhold>0)   //quick press within 3s after long hold
          {
            cruse=1;
          }
          if(DRIVE_MODE==MANUAL_MODE)
          {
            DRIVE_MODE = AUTO_MODE;
//                  LogomaticV2Stat(0, LOGOMATIC_STAT_OFF);
            P0CLR = 1<<GREEN_LED;
          }
          else
          {
            DRIVE_MODE = MANUAL_MODE;
//                  LogomaticV2Stat(0, LOGOMATIC_STAT_ON);
            P0SET = 1<<GREEN_LED;
          }
        }
        else
        {
          sw_state=sw_coast;
          button_longhold=30;
        }
    }
    sw_pre_mode=sw_mode;
}
void WDT_init(void)
{
  #define WDT_COUNT_54p6_us 0xF00D
  #define WDT_COUNT_1p0_ms 0x4650 // (1ms)*(72MHz) / 4
  #define WDT_COUNT_1p5_ms 0x6978 // (1p5ms)*(72MHz) / 4
  #define WDT_COUNT_2_ms 0x8ca0 // (1p5ms)*(72MHz) / 4
  #define WDT_COUNT_10_ms 0x2BF20 // (10ms)*(72MHz) / 4
  #define WDT_COUNT_20_ms 0x57E40 // (20ms)*(72MHz) / 4
  #define WDT_COUNT_MAX 0xFFFFFFFF
  WDTC = WDT_COUNT_2_ms;//0xFFFFFFFF;//WDT_COUNT_10_ms;
  WDMOD = 1<<0 | 1<<1; 
  WDFEED = 0xAA;
  WDFEED = 0x55;
}
void WDT_service(void)
{
  WDFEED = 0xAA;
  WDFEED = 0x55;
}
void Timer_1msTask(void *pvParameters)
{
  sTaskParams_t *  p_Args = reinterpret_cast<sTaskParams_t *>(pvParameters);
  WDT_init();
  while(1) 
  {
    WDT_service();
    vTaskDelay(configTICK_RATE_HZ / (p_Args->frequency));
  }
}

void CountingTask( void * pvParameters )
{
    sTaskParams_t *  p_Args = reinterpret_cast<sTaskParams_t *>(pvParameters);

    float temp_accel,temp_dd;
    float New_SetPoint = 0.0f;

    unsigned int battCap;
    static unsigned int battCapAvg = 807;
    static bool battShutDown = false;
    #define BATT_CAP_THRESH 740//800
    int bat_lev;
    int prev_coast_time;
    int push_delay=0;
    int uart1_data=0;
    char msg[15];
    int crc;
    
    while(true)
    {
      readSwitches();
      battCap = adc_get(&AD1CR, AD_CR_SEL7, &AD1DR7);//getADC1();
      battCapAvg = 7*battCapAvg/8 + battCap/8;
      if (battCapAvg < BATT_CAP_THRESH)
      {
        battShutDown = true;
        P0CLR |= 1<<RED_LED; // turn on LED
      }
      else 
      {
        battShutDown = false;
        P0SET |= 1<<RED_LED;
      }
//      bat_lev = adc_get(&AD1CR, AD_CR_SEL4, &AD1DR7);
//      uart1_data=(DRIVE_MODE<<6)|(sw_mode<<7);
//      uart1_data=(sw_mode<<7);
//      uart1_data|=(DRIVE_MODE<<6);

//      battShutDown = true;
      uart1_data=(sw_mode<<7)|(cruse<<5)|(DRIVE_MODE<<6)|(transientEvent<<4)|(battShutDown<<3);
      if (transientEvent == true)
      {
        if (++transientEventLockoutTimer == 1)
        {
          lock_transientEvent = false;
          transientEventLockoutTimer = 0;
        }
      }
      
//      uart1_tx(0x00);
      for (int i=0;i<6;i++)
      {
        msg[i]=acc_data[i];
      }
      for (int i=6;i<12;i++)
      {
        msg[i]=gyro_data[i-6];  
      }
      msg[4] = (battCap>>8)&0xFF;
      msg[5] = battCap&0xFF;
       msg[12]=uart1_data;
//      msg[12]=0;
      crc=CRC(msg,13);
      msg[13]=(crc>>8)&0xff;
      msg[14]=crc&0xff;
      uart1_tx_array(msg,15);
      uart1_tx(0xff);
      uart1_tx(0x00);

      vTaskDelay(configTICK_RATE_HZ /50);
    //    vTaskDelay(20);
    }
}
void gyroSetTransient(void)
{
  transientEvent = 1;
  lock_transientEvent = true;
  transientEventLockoutTimer = 0;
//  P0CLR |= 1<<RED_LED;
}
void PID_upperTransient(void)
{
//    uint16_t temp = 0;
//    temp = (gyro_data[0]&0xFF)+(gyro_data[1]&0xFF)<<8;
//        temp &= 0xFFFF;
//    if (temp&0x8000 == 0x8000)
//    {  // negative
//      gyro_buff[gyro_buff_index] = temp&0x7FFF - 32768;
////      P0CLR |= 1<<RED_LED;      
//    }
//    else
//    { // positive
//      gyro_buff[gyro_buff_index] = temp&0x7FFF;
////      P0SET |= 1<<RED_LED;
//    }
    
//    gyro_buff[gyro_buff_index]=(signed int)(temp&0x7FFF-temp&0x8000);

   gyro_buff[gyro_buff_index] = gyro_data[0]+gyro_data[1]*256;

//    gyro_buff[gyro_buff_index]=gyro_buff[gyro_buff_index]&0x7FFF-gyro_buff[gyro_buff_index]&0x8000;

//    gyro_buff[gyro_buff_index]=gyro_buff[gyro_buff_index]>500 ? gyro_buff[gyro_buff_index]:0;
//    gyro_buff[gyro_buff_index]=gyro_buff[gyro_buff_index]<-500 ? gyro_buff[gyro_buff_index]:0;
//    gyro_buff_index=gyro_buff_index>=9 ? 0:gyro_buff_index++;
    
//    
    gyro_buffSum=0;
    gyro_buffMin = gyro_buff[gyro_buff_index];
    gyro_buffMax = gyro_buff[gyro_buff_index];

    for (int i =0;i<GYRO_BUFF_SIZE;i++)
    {
       gyro_buffSum = gyro_buffSum + gyro_buff[i];   
       if (gyro_buff[i] > gyro_buffMax)
          gyro_buffMax = gyro_buff[i];
       if (gyro_buff[i] < gyro_buffMin)
          gyro_buffMin = gyro_buff[i];
    }
    gyro_buffDiff = gyro_buffMax - gyro_buffMin;

    if ((gyro_buffDiff > GYRO_DIFF_THRESH) &&
       ((gyro_buffMax > GYRO_MAX_THRESH) || (gyro_buffMin < GYRO_MIN_THRESH)))
    {
        gyroSetTransient();
    }
//    if (gyro_buff[gyro_buff_index]<-3000)
    else if (gyro_buffSum < GYRO_NEG_SUM_THRESH)
    {
//      gyroSetTransient(); 
    }
//    else if (gyro_buff[gyro_buff_index]>25000)
//    {
//      transientEvent = true;
//      P0CLR |= 1<<RED_LED;
//    }
    else
    {
      if (lock_transientEvent == false)
      {
          transientEvent = 0;        
//          P0SET |= 1<<RED_LED;
      }
    }
//    gyro_avg = 7*gyro_avg/8 + gyro_buffSum/8;

//    if (gyro_buffSum<-20000)
//    {
//      transientEvent = true;
//      P0CLR |= 1<<RED_LED;
//    }
// //   else 
////    if (gyro_buffSum>45000)
////    {
////      transientEvent = true;
////      P0CLR |= 1<<RED_LED;
////    }
//    else
//    {
//          transientEvent = false;         
//          P0SET |= 1<<RED_LED;
//    }
    if (++gyro_buff_index >=9)
      gyro_buff_index = 0;
}
void PIDTask( void * pvParameters )
{
    sPIDParams_t *  p_Args = reinterpret_cast<sPIDParams_t *>(pvParameters);
    float Prev_SetPoint = 1.0f;
    float Prev_Integral = 0.0f;
    float stable_SetPoint = 1.0f;
    float stable_Integral = 2.3f;
    float pre_speed;
    

//    for (int i =0;i<data_length;i++)
//    {
//        acc_data_sum[i]=0;
////        acc_data[i]=0;
//        acc_data_mean[i]=0;
//        gyro_data_sum[i]=0;
//        gyro_data[i]=0;
//        gyro_data_mean[i]=0;
//    }  
    for (int i =0;i<GYRO_BUFF_SIZE;i++)
    {
        gyro_buff[i]=0;
    }
//    float acc_x=0.0f;

    i2c_gyro_init();
    delay_ms(1);
    i2c_acc_init();
    delay_ms(1);

    while(true)
    {
      get_gyro_data();
      vTaskDelay(2);
      get_acc_data();
      vTaskDelay(2);
      PID_upperTransient();
    vTaskDelay(configTICK_RATE_HZ /40);
    }
}

int main( void )
{
   // Initialize the logomatic board processor and preipherals
   bootup();
//   initADC();
   adc_init(&PINSEL1, &AD1CR, PINSEL1_P0p22_MASK, PINSEL1_P0p22_ADC1p7, PCONP_ADC1_PWR, (AD_CR_PDN_ON | AD_CR_CLK_DIV_11 | AD1CR_SEL_AD1p7));
//   adc_init(&PINSEL1, &AD1CR, PINSEL0_P0p13_MASK, PINSEL0_P0p13_ADC1p4, PCONP_ADC1_PWR, (AD_CR_PDN_ON | AD_CR_CLK_DIV_11 | AD1CR_SEL_AD1p4));
//   spi_sw_init();
   initTimer1();
    //spi_trans(MEAS);
   // Create the Logging task
   uart_init();
   i2c_init();

   xTaskCreate(CountingTask,
               reinterpret_cast<const signed char *>("TCK"),
               500,
               &CountingArgs,
               COUNTING_TASK_PRIORITY,
               &xCountingTaskHandle
               );
   xTaskCreate(Timer_1msTask,
               reinterpret_cast<const signed char *>("Timer1ms"),
               TASK_STACK_SIZE,
               &Timer1msArgs,
               Timer_1ms_TASK_PRIORITY,
               &xTimer_1msHandle
             );

   // Create the PID controller task
   xTaskCreate(PIDTask,
               reinterpret_cast<const signed char *>("PID"),
               TASK_STACK_SIZE,
               &PIDArgs,
               PID_TASK_PRIORITY,
               &xPIDTaskHandle
               );

#if LOG_DATA
   card_init();
   // Create the Logging task
     xTaskCreate(LoggingTask,
                 reinterpret_cast<const signed char *>("LOG"),
                 TASK_STACK_SIZE,
                 &LoggingArgs,
                 LOGGING_TASK_PRIORITY,
                 &xLoggingTaskHandle
               );
#endif
    //LogomaticV2Stat(STAT0, LOGOMATIC_STAT_OFF);
    //LogomaticV2Stat(STAT1, LOGOMATIC_STAT_ON);
   
   // Start the multitasking environment
   //P0CLR = 1<<GREEN_LED;
   vTaskStartScheduler();
   
   return 0;
} // main

static void I2CISR (void) __attribute__ ((naked));
static void I2CISR (void)
{
portSAVE_CONTEXT ();
I2CISR_handler();
portRESTORE_CONTEXT();
}

void data_cal(void)
{
    acc_x=(  acc_data[0]&0b00001111+acc_data[1]*256 );
    acc_y=(  acc_data[2]&0b00001111+acc_data[3]*256 );
    acc_z=(  acc_data[4]&0b00001111+acc_data[5]*256 );
    gyro_x=( gyro_data[0]+gyro_data[1]*256 );
    gyro_y=(  gyro_data[2]+gyro_data[3]*256 );
    gyro_z=(  gyro_data[4]+gyro_data[5]*256 );
}
float acc_cal_x(void)
  {
    acc_x=(  acc_data[0]&0b00001111+acc_data[1]*256 )/4096.0 *4.0-2;
    return acc_x;
  }
float acc_cal_y(void)
  {
    acc_y=(  acc_data[2]&0b00001111+acc_data[3]*256 )/4096.0 *4.0-2;
    return acc_y;
  }
float acc_cal_z(void)
  {
    acc_z=(  acc_data[4]&0b00001111+acc_data[5]*256 )/4096.0 *4.0-2;
    return acc_z;
  }

void i2c_init(void)
{
  PINSEL0=(PINSEL0 & 0xffffff0f) | 0x50;
  I20SCLH = 200;
  I20SCLL = 200;     //PCLK=12MX6    FRQ=PCLK/(100+100)=360KHZ
 // ISR SETTINGS
   VICIntSelect = 0x00;  //I2C0 
   VICVectCntl1 = 0x20 | 9;    //I2C0
   VICVectAddr1 = ( long )I2CISR;
   VICIntEnable |= ( 1<<9 );   // ISR ENABLE
   VICDefVectAddr=( long )I2CISR;
   PCONP|=1<<7;// I2C POWER ENABLE
   I20ADR=0X72;
   I20CONCLR = 0x6c;
   I20CONSET = 0x40;  //ENABLE
   i2c_bus_busy=0; // i2c bus is not busy
}

void i2c_acc_init(void)
{
  int _temp_i=0;
  i2c_bus_busy=1;    // i2c bus is busy
  I2CAddress = acc_add;
  I2CData = acc_init;
  I2C_reg_Address=CTRL_REG1;
  write_count=1;
//  i2cact=write;
  multi_read=0;
  I20CONCLR = 0x6c;
  I20CONSET = 0x40;  //ENABLE
  I20CONSET = 0x64; //start
}

void i2c_gyro_init(void)
{
  int _temp_i=0;
  i2c_bus_busy=1;    // i2c bus is busy
  I2CAddress = gyro_add;
  I2CData = 0b10001111;
  I2C_reg_Address=0x20;
  write_count=1;
//  i2cact=write;
  multi_read=0;
  I20CONCLR = 0x6c;
  I20CONSET = 0x40;  //ENABLE
  I20CONSET = 0x64; //start
}

void get_acc_data(void)
{
  I2CAddress = acc_add;
  I2C_reg_Address = OUT_X_LSB;
  i2cact=read_acc;
  multi_read=1;
  read_count=0;

  I20CONCLR = 0X6C;

//  delay_ms(1);
  I20CONSET = 0x00000040;   //enable
  I20CONSET = 0x00000020;   //start
}
void get_gyro_data(void)
{
  I2CAddress = gyro_add;
  I2C_reg_Address =OUT_X_GYRO ;
  i2cact=read_gyro;
  multi_read=1;
  read_count=0;

  I20CONCLR = 0X6C;

//  delay_ms(1);
  I20CONSET = 0x00000040;   //enable
  I20CONSET = 0x00000020;   //start
}

void I2CISR_handler (void)
{
  switch (I20STAT&0xf8)
  {
//    case (0x00):   //bus error
//      I20CONSET =0X14;
//    break;
    case (0x08):   //start transmitted
    {
      I20DAT=I2CAddress; 
      I20CONCLR=0X20;  //CLEAR Sta
    }
    break;
    case (0x10):  // repeat start transmitted
    {
      I20DAT=I2CAddress|1; ;  //read
      I20CONCLR=0x20;  // clear start & SI
    }
    break;
    case (0x18):
    {
      I20DAT=I2C_reg_Address; 
    }
    break; 
//    case (0x20):
//   // I20DAT = I2CAddress;
////    acc_data[1]=0x23;
//    
//    break;
    case (0x28):
    {
      if(multi_read==0)   //not multi read
      {
        if (write_count>0)
        {
          I20DAT=I2CData;    //write acc_init to acc sensor
          write_count--;
        }
        else
        {
          I20CONSET = 0x10;  //stop will transmitted
          i2c_bus_busy=0;  // free i2c bus
        }
      }
      else
      {
        I20CONSET = 0x00000020; // repeat start
      }   
    }
    break;
    case (0x40):
    {
      I20CONSET = 1<<2; // AA=1 ACK will be returned
    }
    break;
    case (0x50):
    {
      if(read_count<data_length)
      {
        if(i2cact==read_acc)
        {
          acc_data[read_count]=I20DAT;
        }
        else
        {
          gyro_data[read_count]=I20DAT;  
        }

        if(read_count==(data_length-2))
        {
          I20CONCLR= 1<<2; // AA=0 
        }
        read_count++;
      }
      else
      {
       
      }
    }
    break;
    case (0x58):
    {
      if(i2cact==read_acc)
      {
        acc_data[read_count]=I20DAT;
      }
      else
      {
        gyro_data[read_count]=I20DAT;  
      }         
      I20CONSET = 0x10;  //stop will transmitted
      i2c_bus_busy=0; // free i2c bus
    }       
    break;
    default:
    {
      I20CONSET = 0x10;  //stop will transmitted
      i2c_bus_busy=0; // free i2c bus
//      acc_data[1]=0x23;
    }
    break;
  }
    I20CONCLR = 0X08;
    VICVectAddr=0x00;
}
